# -*- encoding: utf-8 -*-
"""
Edited at 2018-6-21  by Stone
    1. Add CrossRoad.laneGuides and its show and  hide method.
"""
from Shapes import *
from visual import *

# ===== Type definition =====
pos = vector  # to convenient some pos calculation


# ===== Classes =====
class Car(VehicleShape, object):
    """This class is used to provide the geometric and dynamic features of the Cars.
    All the unit is based on SI, i.e. m/s, m/s^2"""
    def __init__(self, orientation, driver=None, init_pos=vector(100,100,-200)):
        super(Car, self).__init__(model='car')
        self.__orientation = orientation  # the unit vector indicates where the car is facing toward
        self.body.axis = self.__orientation * self.body.axis.mag
        self.set_pos(init_pos)
        self.__v = vector(0, 0, 0)
        self.__a = vector(0, 0, 0)
        self.__w = 0  # omega, angular velocity in degree

        self.driver = driver
        self.give_a_driver(driver)

    def move_one_frame(self, delta_t):
        # update status
        self.__v += self.__a * delta_t
        ang = radians(self.__w * delta_t)
        self.body.rotate(angle=ang, axis=scene.up)
        self.__orientation *= self.body.axis.norm()
        self.__a *= self.__orientation
        self.__v *= self.__orientation
        # update positions
        self.body.pos += self.__v * delta_t

    def get_pos(self):
        return self.body.pos

    def get_orientation(self):
        return self.__orientation

    def get_a(self):
        return self.__a

    def get_v(self):
        return self.__v

    def get_w(self):
        return self.__w

    def get_face_to_face_distance(self, anotherCar):
        """To get the distance from the former car's back face to this car's front face."""
        l, w, h = anotherCar.get_size()
        delta_pos = anotherCar.get_pos() - self.__pos
        dis = sqrt(delta_pos[0]**2 + delta_pos[1]**2 + delta_pos[2]**2) - (l + self.__length)/2

        return dis

    def set_pos(self, pos):
        self.body.pos = pos
        # to keep car's buttom stay on the ground
        self.body.pos[2] = self.get_size()['height'] / 2.0

    def set_a(self, a):
        self.__a = a

    def set_orientation(self, orn):
        """The axis should be visual.vector type"""
        self.__orientation = orn

    def set_w(self, w):
        self.__w = w

    def is_it_your_driver(self, theDriver):
        return self.driver == theDriver

    def give_a_driver(self, driver):
        self.driver = driver
        if self.driver and (not self.driver.is_it_your_car(self) ):
            self.driver.give_a_car(self)


class CrossRoad(CrossRoadShape):
    """Construct the shape of the crossroad and provide some attributes of this road."""
    def __init__(self, visual_frame):
        self.f = visual_frame
        CrossRoadShape.__init__(self, self.f)

        self.ifShow_LaneGuides = True
        self.laneGuides = {'x': [], '-x': [], 'y': [], '-y': [], 'visual': []}  # used to guide the vehicles
        self.start = {'x': [], '-x': [], 'y': [], '-y': [], 'visual': []}  # where the vehicles start

        self.init_laneGuides()
        if self.ifShow_LaneGuides: self.show_laneGuides()
        #self.construct_shape()

    def init_laneGuides(self):
        num_of_lanes = 4
        lane_center_bias = self.width / 2.0
        # not clever enough, so manually specify some values intelligently
        k=2
        for i in range(1, num_of_lanes + 1):
            self.start['x'].append(( -k * self.length, -(i * self.width + lane_center_bias),0))
            self.start['-x'].append(( k * self.length,   i * self.width + lane_center_bias, 0))
            self.start['y'].append((   i * self.width + lane_center_bias,  -k * self.length, 0))
            self.start['-y'].append((-(i * self.width + lane_center_bias),  k * self.length, 0))

            # the lanes that go straight
            self.laneGuides[ 'x'].append([( k*self.length, -(i*self.width + lane_center_bias), 0)])  # the guide point
            self.laneGuides['-x'].append([(-k*self.length,   i*self.width + lane_center_bias,  0)])  # -x direction
            self.laneGuides[ 'y'].append([(  i*self.width + lane_center_bias,  k*self.length, 0)])  # y direction, too
            self.laneGuides['-y'].append([(-(i*self.width + lane_center_bias),-k*self.length, 0)])

        # the fifth lanes that turn right
        # the guide points at turn
        self.laneGuides['x'].append([(self.laneGuides['-y'][-1][0][0], -(num_of_lanes * self.width + lane_center_bias),0)])
        self.laneGuides['-x'].append([(self.laneGuides[ 'y'][-1][0][0],  num_of_lanes * self.width + lane_center_bias, 0)])
        self.laneGuides['y'].append([(   num_of_lanes * self.width + lane_center_bias,  self.laneGuides[ 'x'][-1][0][1], 0)])
        self.laneGuides['-y'].append([(-(num_of_lanes * self.width + lane_center_bias), self.laneGuides['-x'][-1][0][1], 0)])
        k = 2
        self.start[ 'x'].append((-k * self.length,-(num_of_lanes * self.width + lane_center_bias), 0))
        self.start['-x'].append(( k * self.length,  num_of_lanes * self.width + lane_center_bias,  0))
        self.start[ 'y'].append((  num_of_lanes * self.width + lane_center_bias, -k * self.length, 0))
        self.start['-y'].append((-(num_of_lanes * self.width + lane_center_bias), k * self.length, 0))
        # the end guide points
        self.laneGuides[ 'x'][-1].append((self.laneGuides['-y'][-1][0][0], -k*self.length, 0))
        self.laneGuides['-y'][-1].append((-k*self.length, self.laneGuides['-x'][-1][0][1], 0))
        self.laneGuides['-x'][-1].append((self.laneGuides[ 'y'][-1][0][0],  k*self.length, 0))
        self.laneGuides[ 'y'][-1].append(( k*self.length, self.laneGuides[ 'x'][-1][0][1], 0))
        for dirc in self.laneGuides:
            for l in self.laneGuides[dirc]:
                for i in range(len(l)):
                    l[i] = vector(l[i])
        for dirc in self.start:
            for i in range(len(self.start[dirc])):
                self.start[dirc][i] = vector(self.start[dirc][i])
        print 'laneGuides: \n', self.laneGuides
        print 'starts: \n', self.start

    def hide_laneGuides(self):
        for obj in self.laneGuides['visual']:
            obj.visible = False

    def show_laneGuides(self):
        if self.laneGuides['visual'] == []:  # time to initialize
            for i in self.laneGuides:
                if i == 'visual': continue
                for j in range(len(self.laneGuides[i])):
                    self.laneGuides['visual'].append(curve(pos=[self.start[i][j]] + self.laneGuides[i][j]))
                    # test 1
                    if j == 4: self.laneGuides['visual'][-1].color = color.yellow
                    # test 1 end
        for obj in self.laneGuides['visual']:
            obj.visible = True


class Driver:
    """Drive the car, which means put the traffic rules in this class."""
    def __init__(self, start, laneGuide, car=None):
        self.__MAX_A = (100.0/3.6) / 10    # (100km/h / 3.6)/10s, max acceleration, expressed in 100km/h speed up
        self.__MAX_V = 60/3.6              # 60 km/h / 3.6, max velocity
        self.__DISTANCE_RANGE = (0.5, 2)   # the distance range between two cars
        self.__k_a = 0.3
        self.__k_w = -60  # right rotate
        self.__target_v = 5  # 60/3.6
        self.__laneGuide = iter(laneGuide)                   # the laneGuide at specific lane
        self.__node = {'from': vector(start), 'to': vector(laneGuide[0])}   # the last node where it from and the next one where it's going to
        self.__target_orientation = self.get_target_orientation()

        self.car = car
        self.give_a_car(car)

    def drive(self, delta_t=0):
        self.control_a()

        #self.watch_a()
        #self.watch_v()
        self.watch_orientation()
        self.watch_distance()
        self.car.move_one_frame(delta_t)

    def control_a(self):
        delta_v = self.__target_v - self.car.get_v().mag
        if delta_v > 0:
            self.car.set_a(self.__k_a * delta_v * self.car.get_orientation().norm())
        else:
            self.car.set_a(vector(0, 0, 0))

    def update_node(self):
        self.__node['from'] = self.__node['to']
        try:
            self.__node['to']   = vector(self.__laneGuide.next())
        except StopIteration:
            # now we get the destination, time to say bye bye
            self.say_goodbye()

    def get_target_orientation(self):
        return self.__node['to'] - self.__node['from']

    def say_goodbye(self):pass

    def watch_a(self):
        if self.car.get_a().mag > self.__MAX_A:
            self.car.set_a(self.__MAX_A * norm(self.car.get_orientation()))

    def watch_v(self):
        if self.car.get_v() > self.__MAX_V:
            self.car.set_a(vector(0, 0, 0))

    def watch_distance(self):
        pass

    def watch_orientation(self):
        w = self.__k_w * signed_diff_angle(self.car.get_orientation(), self.__target_orientation)
        self.car.set_w(w)
        # debug
        print "d_ang:", signed_diff_angle(self.car.get_orientation(), self.__target_orientation),
        print "tar_orn:", self.__target_orientation,

    def is_it_your_car(self, theCar):
        return self.car == theCar

    def give_a_car(self, car):
        self.car = car
        if self.car and (not self.car.is_it_your_driver(self)):
            self.car.give_a_driver(self)


# ===== Datas and Initialization =====
scene = display(title=u"十字路口模拟器2018 - by teleM".encode("gbk"))
scene.fov = pi/8
scene.forward = (0, -1, -1)
scene.up = (0, 0, 1)
crd = CrossRoad(frame())

car_groups = dict([(k, []) for k in crd.laneGuides])

framerate = 10
dt = 1.0 / framerate


# ===== Functions =====
def signed_diff_angle(v1, v2, ref_axis=scene.up):
    """  To calculate the angle with sign of two vectors,
    if v1 is clockwise to v2, then get a positive angle,
    if v1 is counter-clockwise to v2, then get a negative angle.
    The sign is given by the direction related to the ref_axis, which points outward to paper."""
    relative_ang = diff_angle(cross(v1, v2), ref_axis)
    if relative_ang > pi/2.0:
        ang = diff_angle(v1, v2)
    elif relative_ang < pi/2.0:
        ang = -diff_angle(v1, v2)
    else:
        raise ValueError("invalid refer axis:%s, please assign another refer axis" % ref_axis)
    # print "v1:%s, v2:%s, ang:%.4f, rel_ang:%s cr_axis:%s, ref_axis:%s|" % (v1, v2, ang, relative_ang, cross(v1, v2), ref_axis),  # debug

    return ang


# test
arrow(axis=(5, 0, 0), color=(1, 0, 0)), arrow(axis=(0,5,0), color=(0,1,0)), arrow(axis=(0,0,5), color=(0,0,1))
car_groups['x'].append(Car(orientation=vector(1, 0, 0), init_pos=crd.start['x'][4],
                           driver=Driver(crd.start['x'][4], crd.laneGuides['x'][4])))  # debug
c = car_groups['x'][0]


def test():
    car_groups['x'][0].driver.drive(dt)
    print " | orn: %s, a:%s, v:%s, w:%s, dt:%.2f" % (c.get_orientation(), c.get_a(), c.get_v(), c.get_w(), dt)
    if c.body.pos.mag > crd.start['x'][0].mag:
        c.set_pos(crd.start['x'][0])

    rate(framerate)

# ===== Main Loop =====
# ... ...

if __name__ == '__main__':
    while 1:
       test()
